//------------------------------------------------------------------------------
//  Biquad Filter for Pd
//
//  biquadratic~.c
//
//  Filters signal through a general purpose biquad
//
//  Created by Cooper on 10/16/15.
//  Copyright (c) 2015 Cooper Baker. All rights reserved.
//------------------------------------------------------------------------------


// todo
// add 'clear' message

//------------------------------------------------------------------------------
// headers
//------------------------------------------------------------------------------

// main header for pd
#include "m_pd.h"

// utility header for Pd Spectral Toolkit project
#include "utility.h"

// included for float min
#include <limits.h>

// disable compiler warnings on windows
#ifdef NT
#pragma warning( disable : 4244 )
#pragma warning( disable : 4305 )
#endif


//------------------------------------------------------------------------------
// biquadratic_class - pointer to this object's definition
//------------------------------------------------------------------------------
static t_class* biquadratic_class;
static t_class* biquadratic_arg_class;


//------------------------------------------------------------------------------
// filter_shape enum - filter shape names
//------------------------------------------------------------------------------
typedef enum
{
    LOWPASS,
    HIGHPASS,
    BANDPASS,
    NOTCH,
    ALLPASS,
    PEAKING,
    LOWSHELF,
    HIGHSHELF
}
filter_shape;


//------------------------------------------------------------------------------
// biquadratic - data structure holding this object's data
//------------------------------------------------------------------------------
typedef struct biquadratic
{
    // this object - must always be first variable in struct
    t_object object;

    // needed for CLASS_MAINSIGNALIN macro call in biquadratic_tilde_setup
    t_float inlet_1;

    // floats to hold inlets' values
    t_float frequency;
    t_float quality;
    t_float decibels;

    // coefficients
    double a1, a2, b0, b1, b2;

    // filter sample memory
    double x1, x2, y1, y2;

    // the filter shape
    filter_shape shape;

    t_float sample_rate;

} t_biquadratic;


//------------------------------------------------------------------------------
// function prototypes
//------------------------------------------------------------------------------
static void          biquadratic_lowpass     ( t_biquadratic* object );
static void          biquadratic_highpass    ( t_biquadratic* object );
static void          biquadratic_bandpass    ( t_biquadratic* object );
static void          biquadratic_allpass     ( t_biquadratic* object );
static void          biquadratic_notch       ( t_biquadratic* object );
static void          biquadratic_peaking     ( t_biquadratic* object );
static void          biquadratic_lowshelf    ( t_biquadratic* object );
static void          biquadratic_highshelf   ( t_biquadratic* object );
static void          biquadratic_clear       ( t_biquadratic* object );
static inline void   biquadratic_calc        ( t_biquadratic* object );
static t_int*        biquadratic_perform     ( t_int* io );
static t_int*        biquadratic_arg_perform ( t_int* io );
static void          biquadratic_dsp         ( t_biquadratic* object, t_signal** sig );
static void          biquadratic_arg_dsp     ( t_biquadratic* object, t_signal** sig );
static void          biquadratic_shape       ( t_biquadratic* object, t_atom name );
static void*         biquadratic_new         ( t_symbol* selector, t_int items, t_atom* list );
void                 biquadratic_tilde_setup ( void );


//------------------------------------------------------------------------------
// biquadratic_lowpass - sets filter shape to lowpass
//------------------------------------------------------------------------------
static void biquadratic_lowpass( t_biquadratic* object )
{
    object->shape = LOWPASS;
}


//------------------------------------------------------------------------------
// biquadratic_highpass - sets filter shape to highpass
//------------------------------------------------------------------------------
static void biquadratic_highpass( t_biquadratic* object )
{
    object->shape = HIGHPASS;
}


//------------------------------------------------------------------------------
// biquadratic_bandpass - sets filter shape to bandpass
//------------------------------------------------------------------------------
static void biquadratic_bandpass( t_biquadratic* object )
{
    object->shape = BANDPASS;
}


//------------------------------------------------------------------------------
// biquadratic_allpass - sets filter shape to allpass
//------------------------------------------------------------------------------
static void biquadratic_allpass( t_biquadratic* object )
{
    object->shape = ALLPASS;
}


//------------------------------------------------------------------------------
// biquadratic_notch - sets filter shape to notch
//------------------------------------------------------------------------------
static void biquadratic_notch( t_biquadratic* object )
{
    object->shape = NOTCH;
}


//------------------------------------------------------------------------------
// biquadratic_peaking - sets filter shape to peaking
//------------------------------------------------------------------------------
static void biquadratic_peaking( t_biquadratic* object )
{
    object->shape = PEAKING;
}


//------------------------------------------------------------------------------
// biquadratic_lowshelf - sets filter shape to lowshelf
//------------------------------------------------------------------------------
static void biquadratic_lowshelf( t_biquadratic* object )
{
    object->shape = LOWSHELF;
}


//------------------------------------------------------------------------------
// biquadratic_highshelf - sets filter shape to highshelf
//------------------------------------------------------------------------------
static void biquadratic_highshelf( t_biquadratic* object )
{
    object->shape = HIGHSHELF;
}


//------------------------------------------------------------------------------
// biquadratic_clear - clears filter memory
//------------------------------------------------------------------------------
static void biquadratic_clear( t_biquadratic* object )
{
    object->x1 = 0;
    object->x2 = 0;
    object->y1 = 0;
    object->y2 = 0;
}


//------------------------------------------------------------------------------
// biquadratic_calc - calculates the filter coefficients
//
//      Adapted From:
//      Cookbook formulae for audio EQ biquad filter coefficients
//      By Robert Bristow-Johnson
//      http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
//
//------------------------------------------------------------------------------
static inline void biquadratic_calc( t_biquadratic* object )
{
    // intermediate variables
    double quality   = ClipMin( object->quality, C_FLOAT_MIN );
    double omega     = C_2_PI * Clip( object->frequency, C_FLOAT_MIN, object->sample_rate * 0.5 ) / object->sample_rate;
    double cos_omega = Cosine( omega );
    double alpha     = Sine( omega ) / ( 2.0 * quality );
    double amp, beta;
    double a0, a1, a2, b0, b1, b2;

    // calculate based on filter shape
    switch( object->shape )
    {
        case LOWPASS    :   b0 =  ( 1.0 - cos_omega ) * 0.5 ;
                            b1 =    1.0 - cos_omega         ;
                            b2 =  ( 1.0 - cos_omega ) * 0.5 ;
                            a0 =    1.0 + alpha             ;
                            a1 =   -2.0 * cos_omega         ;
                            a2 =    1.0 - alpha             ;
                            break;

        case HIGHPASS   :   b0 =  ( 1.0 + cos_omega ) * 0.5 ;
                            b1 = -( 1.0 + cos_omega )       ;
                            b2 =  ( 1.0 + cos_omega ) * 0.5 ;
                            a0 =    1.0 + alpha             ;
                            a1 =   -2.0 * cos_omega         ;
                            a2 =    1.0 - alpha             ;
                            break;

        case BANDPASS   :   b0 =  alpha * quality ;
                            b1 =  0.0             ;
                            b2 = -alpha * quality ;
                            a0 =  1.0 + alpha     ;
                            a1 = -2.0 * cos_omega ;
                            a2 =  1.0 - alpha     ;
                            break;

        case ALLPASS    :   b0 =  1.0 - alpha     ;
                            b1 = -2.0 * cos_omega ;
                            b2 =  1.0 + alpha     ;
                            a0 =  1.0 + alpha     ;
                            a1 = -2.0 * cos_omega ;
                            a2 =  1.0 - alpha     ;
                            break;

        case NOTCH      :   b0 =  1.0             ;
                            b1 = -2.0 * cos_omega ;
                            b2 =  1.0             ;
                            a0 =  1.0 + alpha     ;
                            a1 = -2.0 * cos_omega ;
                            a2 =  1.0 - alpha     ;
                            break;

        case PEAKING    :   amp =  SquareRoot( Power( 10.0, ( object->decibels * 0.05 ) ) );
                            b0  =  1.0 + ( alpha * amp ) ;
                            b1  = -2.0 *   cos_omega     ;
                            b2  =  1.0 - ( alpha * amp ) ;
                            a0  =  1.0 + ( alpha / amp ) ;
                            a1  = -2.0 *   cos_omega     ;
                            a2  =  1.0 - ( alpha / amp ) ;
                            break;

        case LOWSHELF   :   amp  =  SquareRoot( Power( 10.0, ( object->decibels * 0.05 ) ) );
                            beta =  2.0 * SquareRoot( amp ) * alpha;
                            b0   =        amp * ( ( amp + 1.0 ) - ( amp - 1.0 ) * cos_omega + beta ) ;
                            b1   =  2.0 * amp * ( ( amp - 1.0 ) - ( amp + 1.0 ) * cos_omega        ) ;
                            b2   =        amp * ( ( amp + 1.0 ) - ( amp - 1.0 ) * cos_omega - beta ) ;
                            a0   =                ( amp + 1.0 ) + ( amp - 1.0 ) * cos_omega + beta   ;
                            a1   = -2.0 *       ( ( amp - 1.0 ) + ( amp + 1.0 ) * cos_omega        ) ;
                            a2   =                ( amp + 1.0 ) + ( amp - 1.0 ) * cos_omega - beta   ;
                            break;

        case HIGHSHELF  :   amp  =  SquareRoot( Power( 10.0, ( object->decibels * 0.05 ) ) );
                            beta =  2.0 * SquareRoot( amp ) * alpha;
                            b0   =        amp * ( ( amp + 1.0 ) + ( amp - 1.0 ) * cos_omega + beta ) ;
                            b1   = -2.0 * amp * ( ( amp - 1.0 ) + ( amp + 1.0 ) * cos_omega        ) ;
                            b2   =        amp * ( ( amp + 1.0 ) + ( amp - 1.0 ) * cos_omega - beta ) ;
                            a0   =                ( amp + 1.0 ) - ( amp - 1.0 ) * cos_omega + beta   ;
                            a1   =  2.0 *       ( ( amp - 1.0 ) - ( amp + 1.0 ) * cos_omega        ) ;
                            a2   =                ( amp + 1.0 ) - ( amp - 1.0 ) * cos_omega - beta   ;
                            break;
    }

    // normalize coefficients
    object->b0 = b0 / a0;
    object->b1 = b1 / a0;
    object->b2 = b2 / a0;
    object->a1 = a1 / a0;
    object->a2 = a2 / a0;
}


//------------------------------------------------------------------------------
// biquadratic_perform - the signal processing function of this object
//------------------------------------------------------------------------------
static t_int* biquadratic_perform( t_int* io )
{
    // store variables from dsp input/output array
    t_float*       in1    = ( t_float*       )( io[ 1 ] );
    t_float*       in2    = ( t_float*       )( io[ 2 ] );
    t_float*       in3    = ( t_float*       )( io[ 3 ] );
    t_float*       in4    = ( t_float*       )( io[ 4 ] );
    t_float*       out    = ( t_float*       )( io[ 5 ] );
    t_int          frames = ( t_int          )( io[ 6 ] );
    t_biquadratic* o      = ( t_biquadratic* )( io[ 7 ] );

    // sample i/o temp
    double x, y;

    // signal vector iterator variable
    t_int n = -1;

    // the dsp loop
    while( ++n < frames )
    {
        // store the input sample
        x = in1[ n ];

        // store the parameters
        o->frequency = in2[ n ];
        o->quality   = in3[ n ];
        o->decibels  = in4[ n ];

        // calculate coefficients
        biquadratic_calc( o );

        // perform direct form I biquad calculation
        y = o->b0 * x + o->b1 * o->x1 + o->b2 * o->x2 - o->a1 * o->y1 - o->a2 * o->y2;

        // propagate the filter samples
        o->x2 = o->x1;
        o->x1 = x;
        o->y2 = o->y1;
        o->y1 = y;

        // output the filtered sample
        out[ n ] = y;
    }

    // return the dsp input/output array address plus one more than its size
    // to provide a pointer to the next perform function in pd's call list
    return &( io[ 8 ] );
}


//------------------------------------------------------------------------------
// biquadratic_arg_perform - the signal processing function of this object
//------------------------------------------------------------------------------
static t_int* biquadratic_arg_perform( t_int* io )
{
    // store variables from dsp input/output array
    t_float*       in     = ( t_float*       )( io[ 1 ] );
    t_float*       out    = ( t_float*       )( io[ 2 ] );
    t_int          frames = ( t_int          )( io[ 3 ] );
    t_biquadratic* o      = ( t_biquadratic* )( io[ 4 ] );

    // signal vector iterator variable
    t_int n = -1;

    // sample i/o temp
    double x, y;

    // calculate coefficients
    biquadratic_calc( o );

    // the dsp loop
    while( ++n < frames )
    {
        // store the input sample
        x = in[ n ];

        // perform direct form I biquad calculation
        y = o->b0 * x + o->b1 * o->x1 + o->b2 * o->x2 - o->a1 * o->y1 - o->a2 * o->y2;

        // propagate the filter samples
        o->x2 = o->x1;
        o->x1 = x;
        o->y2 = o->y1;
        o->y1 = y;

        // output the filtered sample
        out[ n ] = y;
    }

    // return the dsp input/output array address plus one more than its size
    // to provide a pointer to the next perform function in pd's call list
    return &( io[ 5 ] );
}


//------------------------------------------------------------------------------
// biquadratic_dsp - installs this object's dsp function in pd's callback list
//------------------------------------------------------------------------------
static void biquadratic_dsp( t_biquadratic* object, t_signal** sig )
{
    object->sample_rate = sig[ 0 ]->s_sr;

    // dsp_add arguments
    //--------------------------------------------------------------------------
    // perform routine
    // number of passed parameters
    // inlet1 sample vector
    // inlet2 sample vector
    // inlet3 sample vector
    // inlet4 sample vector
    // outlet sample vector
    // sample frames to process (vector size)
    // pointer to this object
    dsp_add
    (
        biquadratic_perform,
        7,
        sig[ 0 ]->s_vec,
        sig[ 1 ]->s_vec,
        sig[ 2 ]->s_vec,
        sig[ 3 ]->s_vec,
        sig[ 4 ]->s_vec,
        sig[ 0 ]->s_n,
        object
    );
}


//------------------------------------------------------------------------------
// biquadratic_arg_dsp - installs this object's dsp function in pd's callback list
//------------------------------------------------------------------------------
static void biquadratic_arg_dsp( t_biquadratic* object, t_signal** sig )
{
    object->sample_rate = sig[ 0 ]->s_sr;

    // dsp_add arguments
    //--------------------------------------------------------------------------
    // perform routine
    // number of passed parameters
    // inlet sample vector
    // outlet sample vector
    // sample frames to process (vector size)
    // pointer to this object
    dsp_add
    (
        biquadratic_arg_perform,
        4,
        sig[ 0 ]->s_vec,
        sig[ 1 ]->s_vec,
        sig[ 0 ]->s_n,
        object
    );
}


//------------------------------------------------------------------------------
// biquadratic_shape - sets filter shape based on name string
//------------------------------------------------------------------------------
static void biquadratic_shape( t_biquadratic* object, t_atom name )
{
    const char* shape = name.a_w.w_symbol->s_name;

    if     ( StringMatch( shape, "lowpass"   ) ){ object->shape = LOWPASS   ; }
    else if( StringMatch( shape, "highpass"  ) ){ object->shape = HIGHPASS  ; }
    else if( StringMatch( shape, "bandpass"  ) ){ object->shape = BANDPASS  ; }
    else if( StringMatch( shape, "allpass"   ) ){ object->shape = ALLPASS   ; }
    else if( StringMatch( shape, "notch"     ) ){ object->shape = NOTCH     ; }
    else if( StringMatch( shape, "peaking"   ) ){ object->shape = PEAKING   ; }
    else if( StringMatch( shape, "lowshelf"  ) ){ object->shape = LOWSHELF  ; }
    else if( StringMatch( shape, "highshelf" ) ){ object->shape = HIGHSHELF ; }
    else
    {
        pd_error( object, "biquadratic~: unknown filter type" );
    }
}


//------------------------------------------------------------------------------
// biquadratic_new - instantiates a copy of this object in pd
//------------------------------------------------------------------------------
static void* biquadratic_new( t_symbol* selector, t_int items, t_atom* list )
{
    if( items > 1 )
    {
        // create a pointer to this object
        t_biquadratic* object = ( t_biquadratic* )pd_new( biquadratic_arg_class );

        // create a new signal outlet for this object
        outlet_new( &object->object, gensym( "signal" ) );

        // create float inlets
        floatinlet_new( &object->object, &object->frequency );
        floatinlet_new( &object->object, &object->quality   );
        floatinlet_new( &object->object, &object->decibels  );

        // initialize parameters
        object->shape     = LOWPASS;
        object->frequency = 1000;
        object->quality   = 1;
        object->decibels  = 0;

        // parse initialization arguments
        //----------------------------------------------------------------------
        if( items > 0 )
        {
            if( list[ 0 ].a_type == A_SYMBOL )
            {
                biquadratic_shape( object, list[ 0 ] );
            }
            else
            {
                pd_error( object, "biquadratic~: invalid argument 1 type" );
            }
        }

        if( items > 1 )
        {
            if( list[ 1 ].a_type == A_FLOAT )
            {
                object->frequency = atom_getfloatarg( 1, ( int )items, list );
            }
            else
            {
                pd_error( object, "biquadratic~: invalid argument 2 type" );
            }
        }

        if( items > 2 )
        {
            if( list[ 2 ].a_type == A_FLOAT )
            {
                object->quality = atom_getfloatarg( 2, ( int )items, list );
            }
            else
            {
                pd_error( object, "biquadratic~: invalid argument 3 type" );
            }
        }

        if( items > 3 )
        {
            if( list[ 3 ].a_type == A_FLOAT )
            {
                object->decibels = atom_getfloatarg( 3, ( int )items, list );
            }
            else
            {
                pd_error( object, "biquadratic~: invalid argument 4 type" );
            }
        }

        if( items > 4 )
        {
            pd_error( object, "biquadratic~: extra arguments ignored" );
        }

        return object;
    }
    else
    {
        // create a pointer to this object
        t_biquadratic* object = ( t_biquadratic* )pd_new( biquadratic_class );

        // create signal inlets
        signalinlet_new( &object->object, object->frequency );
        signalinlet_new( &object->object, object->quality   );
        signalinlet_new( &object->object, object->decibels  );

        // create a new signal outlet for this object
        outlet_new( &object->object, gensym( "signal" ) );

        // initialize parameters
        object->shape     = LOWPASS;
        object->frequency = 1000;
        object->quality   = 1;
        object->decibels  = 0;

        // set filter shape based on first argument
        if( items > 0 )
        {
            if( list[ 0 ].a_type == A_SYMBOL )
            {
                biquadratic_shape( object, list[ 0 ] );
            }
            else
            {
                pd_error( object, "biquadratic~: invalid argument 1 type" );
            }
        }

        return object;
    }
}


//------------------------------------------------------------------------------
// biquadratic_tilde_setup - describes the attributes of this object to pd so it may be properly instantiated
// (must always be named with _tilde replacing ~ in the object name)
//------------------------------------------------------------------------------
void biquadratic_tilde_setup( void )
{
    // biquadratic class
    //--------------------------------------------------------------------------

    // creates an instance of this object and describes it to pd
    biquadratic_class = class_new( gensym( "biquadratic~" ), ( t_newmethod )biquadratic_new, 0, sizeof( t_biquadratic ), 0, A_GIMME, 0 );

    // declares leftmost inlet as a signal inlet
    CLASS_MAINSIGNALIN( biquadratic_class, t_biquadratic, inlet_1 );

    // installs biquadratic_dsp so that it will be called when dsp is turned on
    class_addmethod( biquadratic_class, ( t_method )biquadratic_dsp, gensym( "dsp" ), 0 );

    // install filter shape message handlers
    class_addmethod( biquadratic_class, ( t_method )biquadratic_lowpass,   gensym( "lowpass"   ), 0 );
    class_addmethod( biquadratic_class, ( t_method )biquadratic_highpass,  gensym( "highpass"  ), 0 );
    class_addmethod( biquadratic_class, ( t_method )biquadratic_bandpass,  gensym( "bandpass"  ), 0 );
    class_addmethod( biquadratic_class, ( t_method )biquadratic_allpass,   gensym( "allpass"   ), 0 );
    class_addmethod( biquadratic_class, ( t_method )biquadratic_notch,     gensym( "notch"     ), 0 );
    class_addmethod( biquadratic_class, ( t_method )biquadratic_peaking,   gensym( "peaking"   ), 0 );
    class_addmethod( biquadratic_class, ( t_method )biquadratic_lowshelf,  gensym( "lowshelf"  ), 0 );
    class_addmethod( biquadratic_class, ( t_method )biquadratic_highshelf, gensym( "highshelf" ), 0 );

    // instal clear message handler
    class_addmethod( biquadratic_class, ( t_method )biquadratic_clear, gensym( "clear" ), 0 );

    // biquadratic arg class
    //--------------------------------------------------------------------------

    // creates an instance of this object and describes it to pd
    biquadratic_arg_class = class_new( gensym( "biquadratic~" ), 0, 0, sizeof( t_biquadratic ), 0, 0, 0 );

    // declares leftmost inlet as a signal inlet
    CLASS_MAINSIGNALIN( biquadratic_arg_class, t_biquadratic, inlet_1 );

    // installs biquadratic_dsp so that it will be called when dsp is turned on
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_arg_dsp, gensym( "dsp" ), 0 );

    // install filter shape message handlers
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_lowpass,   gensym( "lowpass"   ), 0 );
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_highpass,  gensym( "highpass"  ), 0 );
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_bandpass,  gensym( "bandpass"  ), 0 );
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_allpass,   gensym( "allpass"   ), 0 );
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_notch,     gensym( "notch"     ), 0 );
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_peaking,   gensym( "peaking"   ), 0 );
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_lowshelf,  gensym( "lowshelf"  ), 0 );
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_highshelf, gensym( "highshelf" ), 0 );

    // instal clear message handler
    class_addmethod( biquadratic_arg_class, ( t_method )biquadratic_clear, gensym( "clear" ), 0 );

    // announce this object in the pd console
    Announce( "biquadratic~: biquad filter - v1.0" );
}


//------------------------------------------------------------------------------
// EOF
//------------------------------------------------------------------------------